#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import time

class MoveLineCommander(Node):
    def __init__(self):
        super().__init__('move_line_commander')

        self.pub_move_joint = self.create_publisher(Float64MultiArray, '/command_move_joint', 10)
        self.pub_move_line = self.create_publisher(Float64MultiArray, '/command_move_line', 10)

        # 预设指令
        self.move_joint_cmd = [-1.215456, 0.4159984588623047, 0.4808502197265625,
                               -0.7959480285644531, 1.7736701965332031,
                               -0.8081560134887695, -2.131875991821289]

        self.move_line_cmds = [
            # [0.2, -0.38, 0.32, 0.0, 0.0, 0.0, 0.0],
            # [0.0, -0.38, 0.32, 0.0, 0.0, 0.0, 0.0],
            # [0.0, -0.38, 0.52, 0.0, 0.0, 0.0, 0.0],
            # [0.2, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],
            # [0.18, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],
            # [0.16, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],
            # [0.14, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],
            # [0.12, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],
            # [0.1, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],
            # [0.08, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],
            # [0.06, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],
            # [0.04, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],

            [0.2, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],
            [0.2, -0.38, 0.32, 0.5, 0.5, -0.5, 0.5],
            [0.0, -0.38, 0.32, 0.5, 0.5, -0.5, 0.5],
            [0.0, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],
            [0.2, -0.38, 0.52, 0.5, 0.5, -0.5, 0.5],

        ]

        self.execute_sequence()

    def wait_for_subscribers(self, timeout=1.0):
        start = time.time()
        while time.time() - start < timeout:
            if self.pub_move_joint.get_subscription_count() > 0 and self.pub_move_line.get_subscription_count() > 0:
                return True
            self.get_logger().info("Waiting for subscribers...")
            time.sleep(0.1)
        self.get_logger().warn("Timeout waiting for subscribers.")
        return False

    def execute_sequence(self):
        self.wait_for_subscribers(timeout=1.0)

        # 发布 joint 指令
        msg_joint = Float64MultiArray()
        msg_joint.data = self.move_joint_cmd
        self.pub_move_joint.publish(msg_joint)
        self.get_logger().info("Published move_joint_cmd.")
        time.sleep(3)  # 等待机器人运动完毕

        # 发布多组 line 指令
        for i, cmd in enumerate(self.move_line_cmds):
            msg_line = Float64MultiArray()
            msg_line.data = cmd
            self.pub_move_line.publish(msg_line)
            self.get_logger().info(f"Published move_line_cmd {i+1}")
            time.sleep(1)  # 每条之间延时

def main():
    rclpy.init()
    node = MoveLineCommander()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
